
% 单视图相机标定
function [K,W] = SingleViewCameraCalibration(XZ_line, YZ_line, XY_line)
    % 输入空间三个垂直平面内，互相垂直的多条平行线的点
    % 计算平行线的向量
    linesVec_XZ = GetLinesVector(XZ_line);
    linesVec_YZ = GetLinesVector(YZ_line);
    linesVec_XY = GetLinesVector(XY_line);
    % 计算三个垂向平面内的影消点
    vanish_points_XZ = GetVanishPoints(linesVec_XZ);
    vanish_points_YZ = GetVanishPoints(linesVec_YZ);
    vanish_points_XY = GetVanishPoints(linesVec_XY);
    % 上面两个函数其实内容一样  都是采用的svd分解  (可以用空间知识 叉乘)
    % 影消点构造系数矩阵
    CoeffMat = [GetCoeffVector(vanish_points_XZ,vanish_points_YZ);...
                GetCoeffVector(vanish_points_XZ,vanish_points_XY);...
                GetCoeffVector(vanish_points_YZ,vanish_points_XY)];
    % svd从系数矩阵算出 w向量
    vm = HomoLinearEquationsOptimalSolution(CoeffMat);
    w1 = vm(1);
    w4 = vm(2);
    w5 = vm(3);
    w6 = vm(4);
    % 构造W 矩阵
    W = [w1 0 w4; 0 w1 w5; w4 w5 w6];
    W = W./W(end);
    % cholesky 分解出相机内参
    K = chol(W)^(-1);
    K = K./K(end);
end

% 计算一个平面内 平行线的向量
function linesVec = GetLinesVector(LinePoints_inPlant)
    % 输入一个平面上的相同方向平行线 的两端点 齐次坐标
    % 像素图像上 
    %  线一第一点x 线一第一点y 1
    %  线一第二点x 线一第二点y 1
    %  线二……
    %  即两行确定一条直线
    pointsNums = max(size(LinePoints_inPlant));
    lineNums = floor(pointsNums / 2);
    % nums个点   nums/2条直线
    linesVec = zeros(lineNums,3);
    for i = 1:lineNums
        twoPoints = LinePoints_inPlant(2*i-1:2*i,:); %直线的两端点
        % svd方式求解
        linesVec(i,:) = HomoLinearEquationsOptimalSolution(twoPoints);
    end
end

% 计算平行线向量的影消点
function vanish_point = GetVanishPoints(linesVec)
    % 输入n个平行线的 向量形式
    vm = HomoLinearEquationsOptimalSolution(linesVec);
    vanish_point = vm / vm(end);
end

% 构造齐次线性方程组的一组系数
function vec = GetCoeffVector(points1, points2)
    % 输入 两个影消点 (空间垂直平面内垂直平行线的影消点)
    u1 = points1(1);
    v1 = points1(2);
    z1 = points1(3);
    u2 = points2(1);
    v2 = points2(2);
    z2 = points2(3);

    vec = [u1*u2 + v1*v2, z1*u2+u1*z2, z2*v1+v2*z1, z1*z2];
end


